#!/usr/bin/env python

from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t

gen = ParameterGenerator()

grp_cp = gen.add_group("ControlPoint", type="tab") 
grp_cp.add("speed_fast", double_t, 0, "The max speed with which the control point is moved along the path if the straight distance is larger than speed_fast_threshold", 0.5, 0, 2.0)
grp_cp.add("speed_fast_threshold", double_t, 0, "Min straight distance in front of the robot for fast speed to be used", 1.5, 0, 5.0)
grp_cp.add("speed_fast_threshold_angle", double_t, 0, "Max angle between current pose and pose on path to still count as straight line", 5.0, 0, 45.0)
grp_cp.add("speed_slow", double_t, 0, "Slow speed", 0.2, 0, 2.0)
grp_cp.add("speed_angular", double_t, 0, "The max angular speed with which the control point is moved along the path", 20.0, 1.0, 150.0)
grp_cp.add("acceleration", double_t, 0, "Acceleration for speed transition", 1.0, 0, 10.0)

grp_pid = gen.add_group("PID", type="tab") 
grp_pid.add("kp_lon", double_t, 0, "KP for speed controller lon", 1.0, 0.0, 50.0)
grp_pid.add("ki_lon", double_t, 0, "KI for speed controller lon", 0.0, 0.0, 1.0)
grp_pid.add("ki_lon_max", double_t, 0, "KI windup for speed controller lon", 10.0, 0.0, 100.0)
grp_pid.add("kd_lon", double_t, 0, "KD for speed controller lon", 0.0, 0.0, 1.0)
grp_pid.add("ki_lat", double_t, 0, "KI for speed controller lat", 0.0, 0.0, 5.0)
grp_pid.add("ki_lat_max", double_t, 0, "KI windup for speed controller lat", 10.0, 0.0, 500.0)
grp_pid.add("kp_lat", double_t, 0, "KP for speed controller lat", 1.0, 0.0, 250.0)
grp_pid.add("kd_lat", double_t, 0, "KD for speed controller lat", 0.0, 0.0, 25.0)
grp_pid.add("kp_ang", double_t, 0, "KP for angle controller", 1.0, 0.0, 50.0)
grp_pid.add("ki_ang", double_t, 0, "KI for angle controller", 0.0, 0.0, 5.0)
grp_pid.add("ki_ang_max", double_t, 0, "KI windup for speed controller angle", 10.0, 0.0, 500.0)
grp_pid.add("kd_ang", double_t, 0, "KD for angle controller", 0.0, 0.0, 5.0)

grp_bot = gen.add_group("Robot", type="tab") 
grp_bot.add("max_cmd_vel_speed", double_t, 0, "Max speed to send to the controller", 2.0, 0.0, 5.0)
grp_bot.add("max_cmd_vel_ang", double_t, 0, "Max angular speed to send to the controller", 2.0, 0.0, 5.0)
grp_bot.add("max_goal_distance_error", double_t, 0, "Wait for robot to get closer than max_goal_distance_error to the goal before setting goal as finished", 1.0, 0.0, 3.0)
grp_bot.add("max_goal_angle_error", double_t, 0, "Wait for robot to get a better angle than max_goal_angle_error before setting goal as finished", 10.0, 0.0, 360.0)
grp_bot.add("goal_timeout", double_t, 0, "Timeout for max_goal_distance_error and max_goal_angle_error", 5.0, 0.0, 100.0)
grp_bot.add("max_follow_distance", double_t, 0, "Stop controller if robot is further away than max_follow_distance (i.e. crash detection)", 1.0, 0.0, 3.0)



gen.add("forward_only", bool_t, 0, "Only allow forward driving", True)
gen.add("restore_defaults", bool_t, 0, " Load default config", False)
gen.add("debug_pid", bool_t, 0, " Emit debug_pid topic", False)

# Recovery
grp_recovery = gen.add_group("recovery", type="tab")   
grp_recovery.add("oscillation_recovery",   bool_t,   0,
  "Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards).",
  True) 
grp_recovery.add("oscillation_v_eps", double_t, 0, "Oscillation velocity eps", 5.0, 0.0, 100.0)
grp_recovery.add("oscillation_omega_eps", double_t, 0, "Oscillation omega eps", 5.0, 0.0, 100.0)
grp_recovery.add("oscillation_recovery_min_duration", double_t, 0, "duration until recovery in s", 5.0, 0.0, 100.0)

# Obstacles
grp_obstacles = gen.add_group("obstacles", type="tab")   
grp_obstacles.add("check_obstacles", bool_t, 0, "check and stop for obstacles in path", True)
grp_obstacles.add("obstacle_lookahead", int_t, 0, "how many path segments should be used for collision check", 5,0,20)
grp_obstacles.add("obstacle_footprint", bool_t, 0, "check footprint at actual pose for collision", True)
grp_obstacles.add("debug_obstacle", bool_t, 0, "debug obstacle check as marker array", True)

exit(gen.generate("ftc_local_planner", "ftc_local_planner", "FTCPlanner"))
